﻿using System;
using System.Text;
using System.Drawing;
using System.Buffers;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;

public static partial class NativeAOT
{
    [UnmanagedCallersOnly(EntryPoint = "kabg")]
    public static unsafe void kabg(int n, IntPtr x_ptr, double t, double a, double b, double c, IntPtr y_ptr)
    {
        double* x = (double*)x_ptr.ToPointer();
        double* y = (double*)y_ptr.ToPointer();

        kabg(n, x, t, a, b, c, y);
    }

    /// <summary>
    /// A_B_G滤波
    /// void kabg(int n, double x[], double t, double a, double b, double c, double y[])
    /// </summary>
    /// <param name="n">量测数据点数。</param>
    /// <param name="x">x[n]n个等间隔点上的量测值。</param>
    /// <param name="t">采样周期。</param>
    /// <param name="a">滤波器结构参数Alpha。</param>
    /// <param name="b">滤波器结构参数Beta。</param>
    /// <param name="c">滤波器结构参数Gamma。</param>
    /// <param name="y">y[n]返回n个等间隔点上的滤波估值。</param>
    public static unsafe void kabg(int n, double* x, double t, double a, double b, double c, double* y)
    {
        int i;
        double s1, ss, v1, vv, a1, aa;

        aa = 0.0;
        vv = 0.0;
        ss = 0.0;
        for (i = 0; i <= n - 1; i++)
        {
            s1 = ss + t * vv + t * t * aa / 2.0;
            v1 = vv + t * aa;
            a1 = aa;
            ss = s1 + a * (x[i] - s1);
            y[i] = ss;
            vv = v1 + b * (x[i] - s1);
            aa = a1 + 2.0 * c * (x[i] - s1) / (t * t);
        }
        return;
    }
    /*
    // A_B_G滤波例
      int main()
      { 
          int i;
          double x[150],y[150],z[150];
          double a,b,c,dt,t;
          RND p;
          a=0.271; b=0.0285; c=0.0005; dt=0.04;
          p = RND(0);
             //产生150个均值为0，方差为0.25的高斯白噪声
    for (i=0; i<150; i++)
              y[i]= p.rndg(0.0,0.5);
          for (i=0; i<=149; i++)
          { 
              t=(i+1)*dt;
              z[i]=3.0*t*t-2.0*t+5.0;
              x[i]=z[i]+y[i];
          }
          kabg(150,x,dt,a,b,c,y);
          for (i=0; i<=149; i=i+5)
          { 
              t=(i+1)*dt;
              cout <<"t=" <<t <<"  x(t)=" <<setw(10) <<x[i] <<"  y(t)=" 
                     <<setw(10) <<y[i] <<"  z(t)=" <<setw(10) <<z[i] <<endl;
          }
          return 0;
      }
    */
}

